from PCANBasic import *
import time
from ctypes import *
#import sys

LKMotorCMD = int

LKCMD_GetStatus=LKMotorCMD(0x9A)
LKCMD_ClearErr=LKMotorCMD(0x9B)
LKCMD_GetStatus2=LKMotorCMD(0x9C)
LKCMD_GetStatus3=LKMotorCMD(0x9D)
LKCMD_MultiturnAngleCtrl=LKMotorCMD(0xA4)
LKCMD_GetMultiturnAngle=LKMotorCMD(0x92)
LKCMD_SpeedCtrl=LKMotorCMD(0xA2)
LKCMD_MotorStop=LKMotorCMD(0x81)





class LK_MotorParam (Structure):
    _fields_=[("ID",c_uint),
              ("temp",c_uint),
              ("volt",c_uint),
              ("speed", c_int16),
              ("speedControl",c_int32),
              ("encoder",c_uint16),
              ("encoderRaw",c_uint16),
              ("encoderOffset",c_uint16),
              ("iqControl",c_int16),
              ("iq",c_int16),
              ("motorAngle",c_int64),
              ("circleAngle",c_uint32),
              ("angleControl",c_int32),
              ("spinDirection",c_uint8),
              ("maxSpeed",c_uint16),
              ("angleIncrement",c_int32),
              ("Accel",c_int32),
    ]


class LK_MotorControl:
    PcanHandle = PCAN_USBBUS1
    IsFD = False
    Bitrate = PCAN_BAUD_1M
    DLLFound = False
    BitrateFD = b'f_clock_mhz=20, nom_brp=5, nom_tseg1=2, nom_tseg2=1, nom_sjw=1, data_brp=2, data_tseg1=3, data_tseg2=1, data_sjw=1'   
    CanID = 0
    motorParam=LK_MotorParam(); 
    motorParam.maxSpeed=2000 #5 dps
    connected=0
    
    def __init__(self,_ID):

        self.CanID=_ID
        self.motorParam.ID=_ID
        self.ReceiveDelay=0.005
        
          
        try:
            self.objPCANBasic = PCANBasic()
            self.DLLFound = True
        except :
            print("Unable to find the library: PCANBasic.dll !")
            self.DLLFound = False
            return

        ## Initialization of the selected channel
        self.objPCANBasic.Uninitialize(self.PcanHandle)

        if self.IsFD:
            stsResult = self.objPCANBasic.InitializeFD(self.PcanHandle,self.BitrateFD)
        else:
            stsResult = self.objPCANBasic.Initialize(self.PcanHandle,self.Bitrate)

        if stsResult != PCAN_ERROR_OK:
            print("Can not initialize. Please check the defines in the code.")
            return      
        else:
            self.connected=1
        
    def GetStatus(self):
        msgCanMessage = TPCANMsg()
        msgCanMessage.ID = 0x140+self.CanID
        msgCanMessage.LEN = 8
        msgCanMessage.MSGTYPE = PCAN_MESSAGE_STANDARD
        msgCanMessage.DATA[0] = LKCMD_GetStatus

        self.objPCANBasic.Write(self.PcanHandle, msgCanMessage)
        time.sleep(self.ReceiveDelay)
        stsResult=self.objPCANBasic.Read(self.PcanHandle)
        #print(len(stsResult))
        #print(stsResult[1].DATA[0:-1])
        #print(stsResult[1].DATA[0]==LKCMD_GetStatus.value)
        if stsResult[1].DATA[0]==LKCMD_GetStatus:
             self.motorParam.temp=stsResult[1].DATA[1]
             self.motorParam.volt=stsResult[1].DATA[3]
             #print(stsResult[1].DATA[0])


    def AngleControl(self,angle):
        
        msgCanMessage = TPCANMsg()
        #msgCanMessage.DATA[0:7]=[0]*8
        msgCanMessage.ID = 0x140+self.CanID
        msgCanMessage.LEN = 8
        msgCanMessage.MSGTYPE = PCAN_MESSAGE_STANDARD
        msgCanMessage.DATA[0] = LKCMD_GetStatus

    def GetMultiturnAngle(self):
        
        msgCanMessage = TPCANMsg()
        msgCanMessage.ID = 0x140+self.CanID
        msgCanMessage.LEN = 8
        msgCanMessage.MSGTYPE = PCAN_MESSAGE_STANDARD
        msgCanMessage.DATA[0] = LKCMD_GetMultiturnAngle

        self.objPCANBasic.Write(self.PcanHandle, msgCanMessage)
        #print("getangleCanWriteDone")
        time.sleep(self.ReceiveDelay)
        stsResult=self.objPCANBasic.Read(self.PcanHandle)

        if stsResult[1].DATA[0]==LKCMD_GetMultiturnAngle:
            tempBytes=bytes(stsResult[1].DATA[1:7])
            self.motorParam.motorAngle=int.from_bytes(tempBytes,'little')
            if self.motorParam.motorAngle>2**47:
                self.motorParam.motorAngle=self.motorParam.motorAngle-2**48
            self.objPCANBasic.Reset(self.PcanHandle)
            print(self.motorParam.motorAngle)


    def SetMaxSpeed(self,_maxSpeed):
        self.motorParam.maxSpeed=_maxSpeed

    def Ctrl_MultiturnAngle(self,_angle):
        
        msgCanMessage = TPCANMsg()
        msgCanMessage.ID = 0x140+self.CanID
        msgCanMessage.LEN = 8
        msgCanMessage.MSGTYPE = PCAN_MESSAGE_STANDARD
        msgCanMessage.DATA[0] = LKCMD_MultiturnAngleCtrl 
        msgCanMessage.DATA[2]=int.to_bytes(self.motorParam.maxSpeed,2,'little')[0]
        msgCanMessage.DATA[3]=int.to_bytes(self.motorParam.maxSpeed,2,'little')[1]
        msgCanMessage.DATA[4]=int.to_bytes(_angle,length=4,byteorder='little',signed=True)[0]
        msgCanMessage.DATA[5]=int.to_bytes(_angle,length=4,byteorder='little',signed=True)[1]
        msgCanMessage.DATA[6]=int.to_bytes(_angle,length=4,byteorder='little',signed=True)[2]
        msgCanMessage.DATA[7]=int.to_bytes(_angle,length=4,byteorder='little',signed=True)[3]

        self.objPCANBasic.Write(self.PcanHandle, msgCanMessage)
        #print(msgCanMessage.DATA[0:8])
        time.sleep(self.ReceiveDelay)
        stsResult=self.objPCANBasic.Read(self.PcanHandle)

        #if stsResult[1].DATA[0]==LKCMD_MultiturnAngleCtrl:
            #print(self.motorParam.motorAngle)
        
    def Ctrl_Speed(self,_speed):
        
        msgCanMessage = TPCANMsg()
        msgCanMessage.ID = 0x140+self.CanID
        msgCanMessage.LEN = 8
        msgCanMessage.MSGTYPE = PCAN_MESSAGE_STANDARD
        msgCanMessage.DATA[0] = LKCMD_SpeedCtrl 
        msgCanMessage.DATA[2]=0
        msgCanMessage.DATA[3]=0
        msgCanMessage.DATA[4]=int.to_bytes(_speed,length=4,byteorder='little',signed=True)[0]
        msgCanMessage.DATA[5]=int.to_bytes(_speed,length=4,byteorder='little',signed=True)[1]
        msgCanMessage.DATA[6]=int.to_bytes(_speed,length=4,byteorder='little',signed=True)[2]
        msgCanMessage.DATA[7]=int.to_bytes(_speed,length=4,byteorder='little',signed=True)[3]

        self.objPCANBasic.Write(self.PcanHandle, msgCanMessage)
        #print(msgCanMessage.DATA[0:8])
        time.sleep(self.ReceiveDelay)
        stsResult=self.objPCANBasic.Read(self.PcanHandle)

    def Ctrl_MotorStop(self):
         
        msgCanMessage = TPCANMsg()
        msgCanMessage.ID = 0x140+self.CanID
        msgCanMessage.LEN = 8
        msgCanMessage.MSGTYPE = PCAN_MESSAGE_STANDARD
        msgCanMessage.DATA[0] = LKCMD_MotorStop

        self.objPCANBasic.Write(self.PcanHandle, msgCanMessage)


    def disconnect(self):
        self.objPCANBasic.Reset(self.PcanHandle)
        self.objPCANBasic.Uninitialize(self.PcanHandle)
        self.connected=0


        